#include "Simulation.hpp"

/** \file Simulation.cpp
 *  \author Adrian Schweizer
 *  \created  $Sa 22 Sep 03:38:22 pm CEST 2007 schwadri@SchwadriLaptop$
 *  \modified $Sa 20 Okt 07:11:29 pm CEST 2007 schwadri@SchwadriLaptop$
 */

#include <cmath>
#include <limits>

#include "../core/math/math.hpp"

using namespace world;

Simulation::Simulation(collision_callback c, const Options& o)
:   m_linear_drag_coefficient(o.linear_drag),
    m_quadratic_drag_coefficient(o.quadratic_drag),
    m_epsilon(o.epsilon),
    m_handle_collision(c),
    m_t(0.0)
{ }

void Simulation::Add(Entity* e)
{
    m_insert_list.push_back(e);
}

void Simulation::Remove(Entity* e)
{
    m_remove_list.push_back(e);
}

void Simulation::Update(real dt)
{
    //entity bookkeeping
    //add and remove entities if necessary
    if(!m_insert_list.empty())
    {
        for(std::vector<Entity*>::const_iterator eiter=m_insert_list.begin();eiter!=m_insert_list.end();++eiter)
            m_entities.push_back(*eiter);
        m_insert_list.clear();
    }

    if(!m_remove_list.empty())
    {
        for(std::vector<Entity*>::const_iterator eiter=m_remove_list.begin();eiter!=m_remove_list.end();++eiter)
            m_entities.remove(*eiter);
        m_remove_list.clear();
    }

    for(iterator eiter=m_entities.begin();eiter!=m_entities.end();++eiter)
    {
        //ignore Static objects
        if((*eiter)->Static())
            continue;

        //determine drag force
        real velocity           = norm((*eiter)->XDot());
        if(velocity > std::numeric_limits<real>::epsilon())
        {
            real drag_multiplier    = -(m_constant_drag_coefficient/velocity + m_linear_drag_coefficient + velocity*m_quadratic_drag_coefficient);

            //apply drag force
            (*eiter)->Act((*eiter)->XDot()*drag_multiplier);
        }

        //do time step
        (*eiter)->Update(dt);

        //TEMP: atm make sure that we stay on the 0 plane
        vector3r x = (*eiter)->X();
        x[2] = 0.0;
        (*eiter)->X(x);
    }

    m_t += dt;

    //check all possible entity pairs for collision
    //if we have k Entities this means we have to do k*(k-1)/2 tests!
    //TODO: minimize collision tests using a quadtree
    for(iterator eiter1=m_entities.begin();eiter1!=m_entities.end();++eiter1)
    {
        iterator eiter2=eiter1;
        ++eiter2;
        for(;eiter2!=m_entities.end();++eiter2)
        {
            //ignore collisions if both objects are static
            if((*eiter1)->Static() && (*eiter2)->Static())
                continue;
            
            Geometry*   g1 = (*eiter1)->Geom().get(),
                        *g2 = (*eiter2)->Geom().get();

            if(g1->type == Geometry::SPHERE && g2->type == Geometry::SPHERE)
                DoSphereSphereCollision(*eiter1,*eiter2);

            else if(g1->type == Geometry::BOX && g2->type == Geometry::SPHERE)
                DoBoxSphereCollision(*eiter1,*eiter2);

            else if(g2->type == Geometry::BOX && g1->type == Geometry::SPHERE)
                DoBoxSphereCollision(*eiter2,*eiter1);

            //else ignore.
        }
    }

}

void Simulation::DoSphereSphereCollision(Entity* e1, Entity* e2)
{
    Geometry    *g1 = e1->Geom().get(),
                *g2 = e2->Geom().get();

    vector3r dx = e2->X() - e1->X();
    real dsqr = normsqr(dx);
    real rsqr = (g1->radius + g2->radius) * (g1->radius + g2->radius);
    if(dsqr < rsqr)
    {
        //we have a collision
        real d = std::sqrt(dsqr);
        vector3r n = dx/d;
        Collision c(e1,e2,e1->X()+dx*(1.0+(g1->radius-g2->radius)/d)*0.5,n);
        DoCollision(c);
    }
}

void Simulation::DoBoxSphereCollision(Entity* e1, Entity* e2)
{
    Geometry    *g1 = e1->Geom().get(),
                *g2 = e2->Geom().get();

    vector3r dx = e2->X() - e1->X();
    real dsqr = normsqr(dx);
    real rsqr = (g1->radius + g2->radius) * (g1->radius + g2->radius);

    if(dsqr < rsqr)
    {
        //now we do the more exact test
        //first transform sphere center into box coordinate frame
        const quatr& p = e1->P();
        vector3r x2;
        quatr xq(0.0,dx[0],dx[1],dx[2]);
        xq  = conj(p)*xq*p;
		//FIXME: ,,,
        x2  = vector3r(xq.R_component_2(),xq.R_component_3(),xq.R_component_4());

        const vector3r& extent = g1->extent;
        //determine nearest point to sphere center on box boundary
        using core::math::clamp;
        vector3r x1(clamp(x2[0],-extent[0],extent[0]),clamp(x2[1],-extent[1],extent[1]),clamp(x2[2],-extent[2],extent[2]));
        dx = x2 - x1;
        dsqr = normsqr(dx);

        if(dsqr < g1->radius * g1->radius)
        {
            //yes! we have a collision
            //transform real distance vector back to global coordinates
            xq = quatr(0.0,dx[0],dx[1],dx[2]);
            xq = p*xq*conj(p);
			//FIXME: ,,,
            dx = vector3r(xq.R_component_2(),xq.R_component_3(),xq.R_component_4());
            real d = norm(dx);
            Collision c(e1,e2,e2->X()-dx,dx/d);
            DoCollision(c);
        }
    }
}

void MakeTangentialVectors(const vector3r& n, vector3r& t1, vector3r& t2)
{

    //determine the first tangential-vector who spans the pseudo "separation" plane of the two geometries
    //for this we take the two biggest components of n to perform the cross-product with one of the three base-coordinate axes

    using std::abs;
    if(abs(n[0]) > abs(n[2]))
    {
        if(abs(n[1]) > abs(n[2]))   //n[0] > n[2] && n[1] > n[2]
        {
            //cross(ez,n)
            t1[0] = -n[1];
            t1[1] = n[0];
            t1[2] = 0.0;
        }
        else    //n[0] > n[2] && n[2] > n[1]
        {
            //cross(ey,n)
            t1[0] = n[2];
            t1[1] = 0.0;
            t1[2] = -n[0];
        }
    }
    else    //n[0] < n[2]
    {
        if(abs(n[1]) > abs(n[0]))   // n[2] > n[0] && n[1] > n[0]
        {
            //cross(ex,n)
            t1[0] = 0.0;
            t1[1] = -n[2];
            t1[2] = n[1];
        }
        else    // n[2] > n[1] && n[0] > n[1]
        {
            //cross(ey,n)
            t1[0] = n[2];
            t1[1] = 0.0;
            t1[2] = -n[0];
        }
    }

    t2 = cross(n,t1);
    t1.normalize();
    t2.normalize();
}


Collision::Collision(Entity* e1, Entity* e2, const vector3r& p, const vector3r& normal)
:   entity1(e1),
    entity2(e2),
    position(p),
    n(normal)
{
    MakeTangentialVectors(n,t1,t2);
}

void Simulation::DoCollision(Collision& c)
{

    //first invoke the collision callback routine which has to be supplied by the Game Controller. This is where the game logic takes over.
    //If it returns true the collision is evaluated further
    //TODO: Proper handling of static entities
    if(m_handle_collision(c))
    {
        //preliminary 1 on 1 collision resolution

        //TODO: Also Handle frictional impulses, rolling friction and "bohr" friction
        vector3r v1 = c.entity1->XDot();
        vector3r v2 = c.entity2->XDot();
        real gamma = dot(c.n,v2 - v1);
        //determine collision impulse * n
        vector3r lambda =  c.entity1->Mass()*c.entity2->Mass()*(1+m_epsilon)/(c.entity1->Mass() + c.entity2->Mass()) * gamma * c.n;

        //determine new linear velocites
        v1 -= lambda / c.entity1->Mass();
        v2 += lambda / c.entity2->Mass();

        //TEMP: here we make sure that the objects stay on the ground for now
        v1[2] = 0.0;
        v2[2] = 0.0;

        c.entity1->XDot(v1);
        c.entity2->XDot(v2);

        //determine new angular velocities
        vector3r o1 = c.entity1->Omega() - c.entity1->InertiaInv() * cross(c.position-c.entity1->X(),lambda);
        vector3r o2 = c.entity2->Omega() + c.entity2->InertiaInv() * cross(c.position-c.entity2->X(),lambda);

        //TEMP: make sure that we only rotate around the z-Axis
        o1[0] = 0.0;
        o1[1] = 0.0;

        o2[0] = 0.0;
        o2[1] = 0.0;

        c.entity1->Omega(o1);
        c.entity2->Omega(o2);

    }
}
